/*
 * Texas Instruments IMG video encoder driver test application.
 *
 * Copyright (c) 2019 Texas Instruments Incorporated - http://www.ti.com/
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation version 2.
 *
 * This program is distributed "as is" WITHOUT ANY WARRANTY of any
 * kind, whether express or implied; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <fcntl.h>
#include <stdint.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <signal.h>
#include <poll.h>
#include <unistd.h>
#include <dirent.h>

#include "main.h"

#ifdef DRMMODE
#include <xf86drmMode.h>
#else
#ifdef DRM
#include <drm.h>
#endif /* DRM */
#endif /* DRMMODE */

#define DEBUG
#ifdef DEBUG
#define debug_printf(fmt, arg...) printf("tienc: " fmt, ##arg);
#else
#define debug_printf(fmt, arg...)
#endif

static const struct tienc_format out_formats[] = {
	{
		.name = "NV12",
		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
		.fourcc = V4L2_PIX_FMT_NV12,
		.size_num = 3,
		.size_den = 2,
		.n_planes = 1,
		.bytes_pp = 1,
	},
	{
		.name = "RGBA",
		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
		.fourcc = V4L2_PIX_FMT_RGB32,
		.size_num = 1,
		.size_den = 1,
		.n_planes = 1,
		.bytes_pp = 4,
	}
};

static const struct tienc_format cap_formats[] = {
	{
		.name = "H264",
		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
		.fourcc = V4L2_PIX_FMT_H264,
		.size_num = 2,
		.size_den = 1,
		.n_planes = 1,
		.bytes_pp = 1,
	},
    {
		.name = "H265",
		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
		.fourcc = V4L2_PIX_FMT_HEVC,
		.size_num = 2,
		.size_den = 1,
		.n_planes = 1,
		.bytes_pp = 1,
	}
};

static int queue_buffer(int fd, int index, struct tienc_buffer buffer,
			struct tienc_format format)
{
	struct v4l2_buffer buf;
	struct v4l2_plane buf_planes[MAX_PLANES];
	int ret = 0;

	memset(&buf, 0, sizeof(buf));
	memset(&buf_planes, 0, sizeof(buf_planes));

	if (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE == format.type)
		buf_planes[0].bytesused = format.size;

	if (format.memory == V4L2_MEMORY_MMAP) {
		buf_planes[0].m.mem_offset = buffer.offset;
	} else if (format.memory == V4L2_MEMORY_DMABUF) {
		buf_planes[0].m.fd = buffer.dbuf_fd;
		buf_planes[0].length = buffer.length;
	} else {
		printf("Invalid memory type selected\n");
		return -1;
	}

	buf.index = index;
	buf.type = format.type;
	buf.memory = format.memory;
	buf.m.planes = buf_planes;
	buf.length = format.n_planes;

	ret = ioctl(fd, VIDIOC_QBUF, &buf);
	if (ret < 0)
		printf("%s QBUF buffer %d failed %d: %s\n",
		       (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type) ?
		       ("CAPTURE") : ("OUTPUT"), index, errno, strerror(errno));
	else
		debug_printf("%s QBUF buffer %d success\n",
			     (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type)
			     ? ("CAPTURE") : ("OUTPUT"), index);

	return ret;
}

static int handle_outbuf(int fd, int rdfd, int index,
			 struct tienc_buffer buffer,
			 struct tienc_format format,
			 int n_frames)
{
	int i, ret = 0;
	static int frame_count = 1; /* Start at 1 to send EOS before last frame */
	struct v4l2_encoder_cmd cmd = {0};

    printf("frame_count = %d, n_frames = %d\n", frame_count, n_frames);
    
	if (frame_count == n_frames) {
		cmd.cmd = V4L2_ENC_CMD_STOP;
		cmd.flags = V4L2_ENC_CMD_STOP_AT_GOP_END;
		ret = ioctl(fd, VIDIOC_TRY_ENCODER_CMD, &cmd);
		if (0 > ret)
			printf("handle_outbuf TRY_ENCODER_CMD failed %d %s\n",
					errno, strerror(errno));
		ret = ioctl(fd, VIDIOC_ENCODER_CMD, &cmd);
		if (0 > ret)
			printf("handle_outbuf ENCODER_CMD failed %d %s\n",
					errno, strerror(errno));
		//frame_count++;
	}

	/*if (frame_count > n_frames) {
		debug_printf("All source frames send %d\n", n_frames);
		return 0;
	}*/
    
    printf("++++++++++++++++++++++++++++ 1\n");
        
        // printf("buffer.length = %d\n", buffer.length);  // buffer.length = 783360
        memset(buffer.mapped, 0, buffer.length);

        // ret = (format.height * format.size_num)/format.size_den;
        // printf("------ result = %d ------\n", ret); // ret = (540*3/2) = 810
        for(i = 0; i < (format.height * format.size_num)/format.size_den; i++) {    // i < 810
            ret = read(rdfd, (buffer.mapped + (i * format.stride)), format.width * format.bytes_pp); // stride=960,810*960=777600
            if (!ret) {
                debug_printf("read returned 0, exiting handle_outbuf\n");
                return 0;
            }
        }

    printf("++++++++++++++++++++++++++++ 2\n");

#if 0
	debug_printf("%s printing contents of buffer %d\n", __func__, index);
	for (i = 0; i < 2400; i = i + 8) {
		debug_printf("0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x,\n",
			     ((char*)buffer.mapped)[i+0],
			     ((char*)buffer.mapped)[i+1],
			     ((char*)buffer.mapped)[i+2],
			     ((char*)buffer.mapped)[i+3],
			     ((char*)buffer.mapped)[i+4],
			     ((char*)buffer.mapped)[i+5],
			     ((char*)buffer.mapped)[i+6],
			     ((char*)buffer.mapped)[i+7]);
	}
#endif
	ret = queue_buffer(fd, index, buffer, format);

	frame_count++;

	return ret;
}

static int handle_capbuf(int fd, int wrfd, int index,
			 struct tienc_buffer buffer,
			 struct tienc_format format,
			 int write_length)
{
	int ret = 0;

	if (write_length)
		ret = write(wrfd, buffer.mapped, write_length);
	if (0 > ret)
		printf("write returned error: %d: %s\n", errno,
		       strerror(errno));

	ret = queue_buffer(fd, index, buffer, format);

	return ret;
}

static int streaming_loop(int fd, int rdfd, int wrfd,
			  struct tienc_buffer outbuffers[],
			  struct tienc_buffer capbuffers[],
			  int n_outbuffers, int n_capbuffers,
			  struct tienc_format out_format,
			  struct tienc_format cap_format,
			  int n_frames)
{
	struct v4l2_buffer buf;
	struct v4l2_plane buf_planes[MAX_PLANES];
	struct pollfd pfd;
	int type, i, ret = 0;
	uint32_t flags = 0;
	struct v4l2_event event;

	debug_printf("%s Enter\n", __func__);

    debug_printf("%s ....\n", "aaaaaaaaaaaaaaaaaaaaaa");

	for (i = 0; i < n_outbuffers; i++)
		handle_outbuf(fd, rdfd, i, outbuffers[i], out_format, n_frames);
    debug_printf("%s ....\n", "bbbbbbbbbbbbbbbbbbbbbbb");

	for (i = 0; i < n_capbuffers; i++)
		handle_capbuf(fd, wrfd, i, capbuffers[i], cap_format, 0);
    debug_printf("%s ....\n", "cccccccccccccccccccccccccc");

	type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	ret = ioctl(fd, VIDIOC_STREAMON, &type);
	if (ret) {
		printf("OUTPUT VIDIOC_STREAMON failed %d: %s\n", errno,
		       strerror(errno));
		return ret;
	}
	debug_printf("OUTPUT VIDIOC_STREAMON succeeded\n");

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	ret = ioctl(fd, VIDIOC_STREAMON, &type);
	if (ret) {
		printf("CAPTURE VIDIOC_STREAMON failed %d: %s\n", errno,
		       strerror(errno));
		return ret;
	}
	debug_printf("CAPTURE VIDIOC_STREAMON succeeded\n");

	pfd.fd = fd;
	pfd.events = POLLIN | POLLOUT | POLLPRI;

	while(!(flags & V4L2_BUF_FLAG_LAST)) {
		/* Poll for any event for 100ms */
		pfd.revents = 0;
		ret = poll(&pfd, 1, 100);

		if (0 > ret) {
			printf("poll had an error %d: %s\n",
			       errno, strerror(errno));
		} else if (0 < ret) {
			if (pfd.revents & POLLOUT) {
				while(1) {
					memset(&buf, 0, sizeof(buf));
					memset(&buf_planes, 0, sizeof(buf_planes));
					buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
					buf.memory = out_format.memory;
					buf.m.planes = buf_planes;
					buf.length = out_format.n_planes;
					ret = ioctl(fd, VIDIOC_DQBUF, &buf);
					if (ret < 0) {
						if (EAGAIN != errno) {
							printf("OUTPUT VIDIOC_DQBUF failed %d: %s\n",
							       errno,
							       strerror(errno));
						} else {
							debug_printf("OUTPUT EAGAIN\n");
							break;
						}
					} else {
						handle_outbuf(fd, rdfd, buf.index,
							      outbuffers[buf.index],
							      out_format,
							      n_frames);
					}
				}
			}
			if (pfd.revents & POLLIN) {
				while(1) {
					memset(&buf, 0, sizeof(buf));
					memset(&buf_planes, 0, sizeof(buf_planes));
					buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
					buf.memory = cap_format.memory;
					buf.m.planes = buf_planes;
					buf.length = cap_format.n_planes;
					ret = ioctl(fd, VIDIOC_DQBUF, &buf);
					if (ret < 0) {
						if (EAGAIN != errno) {
							printf("CAPTURE VIDIOC_DQBUF failed %d: %s\n",
							       errno,
							       strerror(errno));
							if (EPIPE == errno) {
								printf("Got EPIPE, exiting\n");
								flags |= V4L2_BUF_FLAG_LAST;
								break;
							}
						} else {
							debug_printf("CAPTURE EAGAIN\n");
							break;
						}
					} else {
						handle_capbuf(fd, wrfd, buf.index,
							      capbuffers[buf.index],
							      cap_format,
							      buf.m.planes[0].bytesused);
						flags = buf.flags;
						debug_printf("CAPTURE VIDIOC_DQBUF buffer %d flags=%#08x FLAG_LAST=%#08x\n",
							     buf.index, flags,
							     V4L2_BUF_FLAG_LAST);
						if (buf.flags & V4L2_BUF_FLAG_LAST)
							break;
					}
				}
			}
			if (pfd.revents & POLLPRI) {
				memset(&event, 0, sizeof(event));
				ret = ioctl(fd, VIDIOC_DQEVENT, &event);
				if (ret < 0) {
					printf("VIDIOC_DQEVENT failed %d: %s\n",
							errno, strerror(errno));
				} else if (event.type == V4L2_EVENT_EOS) {
					debug_printf("GOT EOS EVENT\n");
				} else {
					printf("VIDIOC_DQEVENT got unexpected event %d\n",
					       event.type);
				}
			}
		}
        else {
            break;
        }
	}

	type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	ret = ioctl(fd, VIDIOC_STREAMOFF, &type);
	if (ret)
		printf("OUTPUT VIDIOC_STREAMOFF failed %d: %s\n", errno,
		       strerror(errno));

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	ret = ioctl(fd, VIDIOC_STREAMOFF, &type);
	if (ret)
		printf("CAPTURE VIDIOC_STREAMOFF failed %d: %s\n", errno,
		       strerror(errno));

	return 0;
}

#ifdef DRM
static int create_drm_buffer(int drmfd, struct tienc_buffer *b,
			     struct tienc_format format)
{
	struct drm_mode_create_dumb gem;
	struct drm_prime_handle prime;
	struct drm_mode_map_dumb gem_map;
	int ret;

	memset(&gem, 0, sizeof gem);
	gem.width = format.stride;
	gem.height = (format.height * format.size_num) / format.size_den;
	gem.bpp = 8 * format.bytes_pp;

	ret = ioctl(drmfd, DRM_IOCTL_MODE_CREATE_DUMB, &gem);
	if (ret) {
		printf("DRM_IOCTL_MODE_CREATE_DUMB failed %d: %s\n",
		       errno, strerror(errno));
		return ret;
	}

	b->bo_handle = gem.handle;

	memset(&prime, 0, sizeof prime);
	prime.handle = b->bo_handle;

	ret = ioctl(drmfd, DRM_IOCTL_PRIME_HANDLE_TO_FD, &prime);
	if (ret) {
		printf("DRM_IOCTL_PRIME_HANDLE_TO_FD failed %d: %s\n",
		       errno, strerror(errno));
		return ret;
	}
	b->dbuf_fd = prime.fd;

	memset(&gem_map, 0, sizeof(gem_map));
	gem_map.handle = gem.handle;

	ret = ioctl(drmfd, DRM_IOCTL_MODE_MAP_DUMB, &gem_map);
	if (ret) {
		printf("DRM_IOCTL_MODE_MAP_DUMB failed %d: %s\n",
		       errno, strerror(errno));
		return ret;
	}

	if (format.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
		b->mapped = mmap(NULL, (size_t)gem.size, PROT_READ,
				 MAP_SHARED, drmfd, gem_map.offset);
	} else if (format.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
		b->mapped = mmap(NULL, (size_t)gem.size, PROT_WRITE,
				 MAP_SHARED, drmfd, gem_map.offset);
	} else {
	}

	if (MAP_FAILED == b->mapped) {
		printf("mmap of %s GEM buffer failed %d: %s\n",
		       (format.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
		       ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
		return -1;
	}
	b->offset = gem_map.offset;
	b->length = gem.size;

	return 0;
}
#endif

static int create_mmap_buffer(int fd, struct tienc_buffer *b,
			      enum v4l2_buf_type type, int index,
			      struct tienc_format format)
{
	struct v4l2_buffer buf;
	struct v4l2_plane buf_planes[MAX_PLANES];

	memset(&buf, 0, sizeof(buf));
	buf.type = type;
	buf.index = index;
	buf.m.planes = buf_planes;
	buf.length = 1;

	if (ioctl(fd, VIDIOC_QUERYBUF, &buf)) {
		printf("VIDIOC_QUERYBUF %s %d failed: %d: %s\n",
		       (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
		       ("CAPTURE") : ("OUTPUT"), index, errno,
		       strerror(errno));
		return -1;
	}

	debug_printf("QUERYBUF %s: buffer %d length (planes)=%d length=%d offset=%d\n",
		     (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
		     ("CAPTURE") : ("OUTPUT"), index, buf.length,
		     buf.m.planes[0].length,
		     buf.m.planes[0].data_offset);

	/*
	 * Required size for CAPTURE can be more finely tuned by driver.
	 * Our calculated size for CAPTURE is a worst case scenario, so allow
	 * driver to take priority.
	 */
	if ((type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) &&
	    (buf.m.planes[0].length < format.size)) {
		printf("Buffer length %d less than required %d\n",
		       buf.m.planes[0].length, format.size);
		return -1;
	}

	b->mapped = mmap(NULL, buf.m.planes[0].length,
				 PROT_READ | PROT_WRITE, MAP_SHARED,
				 fd, buf.m.planes[0].m.mem_offset);
	b->offset = buf.m.planes[0].m.mem_offset;
	b->length = buf.m.planes[0].length;

	debug_printf("After mmap, buffers[%d].mapped = 0x%p\n", index,
		     b->mapped);

	if (MAP_FAILED == b->mapped) {
		printf("mmap of %s buffer failed %d: %s\n",
		       (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
		       ("CAPTURE") : ("OUTPUT"), errno,
		       strerror(errno));
		return -1;
	}
	return 0;
}

static int alloc_bufs(int fd, int drmfd, int *n_buffers,
		      struct tienc_buffer buffers[], enum v4l2_buf_type type,
		      struct tienc_format format)
{
	int i, max, ret = 0;
	struct v4l2_requestbuffers reqbuf = {0};
	debug_printf("%s Enter\n", __func__);

	reqbuf.count = *n_buffers;
	reqbuf.type = type;
	reqbuf.memory = format.memory;

	ret = ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
	if (ret) {
		printf("VIDIOC_REQBUF %s failed %d: %s\n",
		       (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
		       ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
		return ret;
	}

	debug_printf("After VIDIOC_REQBUFS %s getting buf_cnt %d\n",
		     (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
		     ("CAPTURE") : ("OUTPUT"), reqbuf.count);

	*n_buffers = reqbuf.count;

	if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
		max = MAX_CAPBUFS;
	} else if (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE == type) {
		max = MAX_OUTBUFS;
	} else {
		printf("Invalid type %#08x chosen\n", type);
		return -1;
	}

	if (max < *n_buffers) {
		printf("Driver returned more %s buffers %d than MAX %d\n",
		       (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
		       ("CAPTURE") : ("OUTPUT"), *n_buffers, max);
		return -1;
	}

	for (i = 0; i < *n_buffers; i++) {
		if (format.memory == V4L2_MEMORY_MMAP) {
			ret = create_mmap_buffer(fd, &buffers[i], type, i, format);
			if (ret)
				return ret;
		} else if (format.memory == V4L2_MEMORY_DMABUF) {
#ifdef DRM
			ret = create_drm_buffer(drmfd, &buffers[i], format);
			if (ret)
				return ret;
#endif
		}
	}

	return 0;
}

static void dealloc_bufs(int fd, int drmfd, int *n_buffers, struct tienc_buffer buffers[],
			 struct tienc_format format)
{
#ifdef DRM
	struct drm_mode_destroy_dumb gem_destroy;
#endif
	int i, ret = 0;

	debug_printf("%s Enter\n", __func__);

	for (i = 0; i < *n_buffers; i++) {
		if (NULL == buffers[i].mapped) {
			debug_printf("buffer %d not mapped, skip munmap\n", i);
			continue;
		}
		debug_printf("munmap buffer %d mapped=0x%p length =%d\n",
			     i, buffers[i].mapped, buffers[i].length);
		ret = munmap(buffers[i].mapped, buffers[i].length);
		if (ret)
			printf("munmap failed for buffer %d. %d: %s\n",
			       i, errno, strerror(errno));
		buffers[i].mapped = NULL;

#ifdef DRM
		if (format.memory == V4L2_MEMORY_DMABUF) {
			memset(&gem_destroy, 0, sizeof(gem_destroy));
			gem_destroy.handle = buffers[i].bo_handle;
			ret = ioctl(drmfd, DRM_IOCTL_MODE_DESTROY_DUMB,
				    &gem_destroy);
			if (ret)
				printf("DRM_IOCTL_MODE_DESTROY_DUMB failed for buffer %d. %d: %s\n",
				       i, errno, strerror(errno));
		}
#endif
	}
}

static void print_v4l2_format(struct v4l2_format *fmt)
{
	int i;

	debug_printf("type = %d\n", fmt->type);
	debug_printf("width=%d height=%d\n", fmt->fmt.pix_mp.width,
		     fmt->fmt.pix_mp.height);
	debug_printf("pixelformat=%d\n", fmt->fmt.pix_mp.pixelformat);
	debug_printf("field=%d colorspace=%d\n", fmt->fmt.pix_mp.field,
		     fmt->fmt.pix_mp.colorspace);
	for (i = 0; i < fmt->fmt.pix_mp.num_planes; i++) {
		debug_printf("plane_fmt[%d].sizeimage=%d\n", i,
			     fmt->fmt.pix_mp.plane_fmt[i].sizeimage);
		debug_printf("plane_fmt[%d].bytesperline=%d\n", i,
			     fmt->fmt.pix_mp.plane_fmt[i].bytesperline);
	}
	debug_printf("num_planes=%d flags=%d\n", fmt->fmt.pix_mp.num_planes,
			fmt->fmt.pix_mp.flags);
}

static void get_format(int fd, struct tienc_format format)
{
	struct v4l2_format fmt = {0};

	debug_printf("%s Enter\n", __func__);

	fmt.type = format.type;

	if (ioctl(fd, VIDIOC_G_FMT, &fmt)) {
		printf("VIDIOC_G_FMT %s errno %d: %s\n",
		       (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type) ?
		       ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
		return;
	}

	debug_printf("%s printing returned v4l2_format\n", __func__);
	print_v4l2_format(&fmt);
}

static int try_format(int fd, struct tienc_format format)
{
	struct v4l2_format fmt = {0};
	int ret = 0;

	debug_printf("%s Enter\n", __func__);

	fmt.type = format.type;
	fmt.fmt.pix_mp.width = format.width;
	fmt.fmt.pix_mp.height = format.height;
	fmt.fmt.pix_mp.pixelformat = format.fourcc;
	fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
	fmt.fmt.pix_mp.colorspace = V4L2_COLORSPACE_DEFAULT;
	fmt.fmt.pix_mp.plane_fmt[0].sizeimage = format.size;
	fmt.fmt.pix_mp.plane_fmt[0].bytesperline = format.stride;
	fmt.fmt.pix_mp.num_planes = format.n_planes;

	ret = ioctl(fd, VIDIOC_TRY_FMT, &fmt);
	if (ret) {
		printf("VIDIOC_TRY_FMT %s errno %d: %s\n",
		       (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type) ?
		       ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
		return ret;
	}

	debug_printf("%s printing returned v4l2_format\n", __func__);
	print_v4l2_format(&fmt);
	return 0;
}

static int set_format(int fd, struct tienc_format format)
{
	struct v4l2_format fmt = {0};
	int ret = 0;

	debug_printf("%s Enter\n", __func__);

	fmt.type = format.type;
	fmt.fmt.pix_mp.width = format.width;
	fmt.fmt.pix_mp.height = format.height;
	fmt.fmt.pix_mp.pixelformat = format.fourcc;
	fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
	fmt.fmt.pix_mp.colorspace = V4L2_COLORSPACE_DEFAULT;
	fmt.fmt.pix_mp.plane_fmt[0].sizeimage = format.size;
	fmt.fmt.pix_mp.plane_fmt[0].bytesperline = format.stride;
	fmt.fmt.pix_mp.num_planes = format.n_planes;

	ret = ioctl(fd, VIDIOC_S_FMT, &fmt);
	if (ret) {
		printf("VIDIOC_S_FMT %s errno %d: %s\n",
		       (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type) ?
		       ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
		return ret;
	}

	debug_printf("%s printing returned v4l2_format\n", __func__);
	print_v4l2_format(&fmt);
	return 0;
}

static void get_parameters(int fd)
{
	struct v4l2_streamparm parm = {0};

	parm.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	parm.parm.output.capability = V4L2_CAP_TIMEPERFRAME;

	if (ioctl(fd, VIDIOC_G_PARM, &parm)) {
		printf("VIDIOC_G_PARM failed %d %s\n", errno, strerror(errno));
		return;
	}

	debug_printf("VIDIOC_G_PARM got back framerate %d / %d = %d\n",
		     parm.parm.output.timeperframe.denominator,
		     parm.parm.output.timeperframe.numerator,
		     (parm.parm.output.timeperframe.denominator /
		      parm.parm.output.timeperframe.numerator));

	return;
}

static int set_parameters(int fd, int num, int den)
{
	struct v4l2_streamparm parm = {0};

	parm.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	parm.parm.output.capability = V4L2_CAP_TIMEPERFRAME;
	parm.parm.output.timeperframe.numerator = num;
	parm.parm.output.timeperframe.denominator = den;

	if (ioctl(fd, VIDIOC_S_PARM, &parm)) {
		printf("VIDIOC_S_PARM failed %d %s\n", errno, strerror(errno));
		return 1;
	}

	debug_printf("VIDIOC_S_PARM got back framerate %d / %d = %d\n",
		     parm.parm.output.timeperframe.denominator,
		     parm.parm.output.timeperframe.numerator,
		     (parm.parm.output.timeperframe.denominator /
		      parm.parm.output.timeperframe.numerator));

	return 0;
}

static void query_ctrls(int fd)
{
	struct v4l2_queryctrl ctrl;
	struct v4l2_query_ext_ctrl ext_ctrl;

	debug_printf("%s Enter\n", __func__);

	printf("%s Enumerating all non-compound\n", __func__);
	memset(&ctrl, 0, sizeof(ctrl));
	ctrl.id = 0;
	ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
	while (! ioctl(fd, VIDIOC_QUERYCTRL, &ctrl)) {
		printf("%s Got ctrl.id=%#08x type=%d\n",
		       __func__, ctrl.id, ctrl.type);
		printf("%s\tname=%s minimum=%d maximum=%d\n",
		       __func__, ctrl.name, ctrl.minimum, ctrl.maximum);
		printf("%s\tstep=%d default_value=%d\n",
		       __func__, ctrl.step, ctrl.default_value);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
		ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
	}
	printf("%s Enumerating all non-compound ended errno=%d %s\n",
			__func__, errno, strerror(errno));

	printf("%s Enumerating all non-compound ext\n", __func__);
	memset(&ext_ctrl, 0, sizeof(ext_ctrl));
	ext_ctrl.id = 0;
	ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
	while (! ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
		printf("%s Got ext_ctrl.id=%#08x type=%d\n",
		       __func__, ext_ctrl.id, ext_ctrl.type);
		printf("%s\tname=%s minimum=%lld maximum=%lld\n",
		       __func__, ext_ctrl.name, ext_ctrl.minimum,
		       ext_ctrl.maximum);
		printf("%s\tstep=%lld default_value=%lld\n",
		       __func__, ext_ctrl.step, ext_ctrl.default_value);
		printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
		       __func__, ext_ctrl.elem_size, ext_ctrl.elems,
		       ext_ctrl.nr_of_dims);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
		ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
	}
	printf("%s Enumerating all non-compound ext ended errno=%d %s\n",
			__func__, errno, strerror(errno));

	printf("%s Enumerating all compound ext\n", __func__);
	memset(&ext_ctrl, 0, sizeof(ext_ctrl));
	ext_ctrl.id = 0;
	ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_COMPOUND;
	while (! ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
		printf("%s Got ext_ctrl.id=%#08x type=%d\n",
		       __func__, ext_ctrl.id, ext_ctrl.type);
		printf("%s\tname=%s minimum=%lld maximum=%lld\n",
		       __func__, ext_ctrl.name, ext_ctrl.minimum,
		       ext_ctrl.maximum);
		printf("%s\tstep=%lld default_value=%lld\n",
		       __func__, ext_ctrl.step, ext_ctrl.default_value);
		printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
		       __func__, ext_ctrl.elem_size, ext_ctrl.elems,
		       ext_ctrl.nr_of_dims);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
		ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_COMPOUND;
	}
	printf("%s Enumerating all compound ext ended errno=%d %s\n",
	       __func__, errno, strerror(errno));

	printf("%s Enumerating all controls ext\n", __func__);
	memset(&ext_ctrl, 0, sizeof(ext_ctrl));
	ext_ctrl.id = 0;
	ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
	ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_COMPOUND;
	while (! ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
		printf("%s Got ext_ctrl.id=%#08x type=%d\n",
		       __func__, ext_ctrl.id, ext_ctrl.type);
		printf("%s\tname=%s minimum=%lld maximum=%lld\n",
		       __func__, ext_ctrl.name, ext_ctrl.minimum,
		       ext_ctrl.maximum);
		printf("%s\tstep=%lld default_value=%lld\n",
		       __func__, ext_ctrl.step, ext_ctrl.default_value);
		printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
		       __func__, ext_ctrl.elem_size, ext_ctrl.elems,
		       ext_ctrl.nr_of_dims);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
		ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
		ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_COMPOUND;
	}
	printf("%s Enumerating all controls ext ended errno=%d %s\n",
	       __func__, errno, strerror(errno));

	/* Query individual controls */
	memset(&ctrl, 0, sizeof(ctrl));
	ctrl.id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	if (ioctl(fd, VIDIOC_QUERYCTRL, &ctrl)) {
		printf("%s error querying V4L2_CID_MPEG_VIDEO_GOP_SIZE errno=%d %s\n",
		       __func__, errno, strerror(errno));
	} else {
		printf("%s Got ctrl.id=%#08x type=%d\n",
		       __func__, ctrl.id, ctrl.type);
		printf("%s\tname=%s minimum=%d maximum=%d\n",
		       __func__, ctrl.name, ctrl.minimum, ctrl.maximum);
		printf("%s\tstep=%d default_value=%d\n",
		       __func__, ctrl.step, ctrl.default_value);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
	}

	memset(&ctrl, 0, sizeof(ctrl));
	ctrl.id = V4L2_CID_MPEG_VIDEO_BITRATE;
	if (ioctl(fd, VIDIOC_QUERYCTRL, &ctrl)) {
		printf("%s error querying V4L2_CID_MPEG_VIDEO_BITRATE errno=%d %s\n",
		       __func__, errno, strerror(errno));
	} else {
		printf("%s Got ctrl.id=%#08x type=%d\n",
		       __func__, ctrl.id, ctrl.type);
		printf("%s\tname=%s minimum=%d maximum=%d\n",
		       __func__, ctrl.name, ctrl.minimum, ctrl.maximum);
		printf("%s\tstep=%d default_value=%d\n",
		       __func__, ctrl.step, ctrl.default_value);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
	}

	memset(&ctrl, 0, sizeof(ctrl));
	ctrl.id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
	if (ioctl(fd, VIDIOC_QUERYCTRL, &ctrl)) {
		printf("%s error querying V4L2_CID_MPEG_VIDEO_H264_I_PERIOD errno=%d %s\n",
		       __func__, errno, strerror(errno));
	} else {
		printf("%s Got ctrl.id=%#08x type=%d\n",
		       __func__, ctrl.id, ctrl.type);
		printf("%s\tname=%s minimum=%d maximum=%d\n",
		       __func__, ctrl.name, ctrl.minimum, ctrl.maximum);
		printf("%s\tstep=%d default_value=%d\n",
		       __func__, ctrl.step, ctrl.default_value);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
	}

	/* Query individual extended controls */
	memset(&ext_ctrl, 0, sizeof(ext_ctrl));
	ext_ctrl.id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	if (ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
		printf("%s error querying ext V4L2_CID_MPEG_VIDEO_GOP_SIZE errno=%d %s\n",
		       __func__, errno, strerror(errno));
	} else {
		printf("%s Got ext_ctrl.id=%#08x type=%d\n",
		       __func__, ext_ctrl.id, ext_ctrl.type);
		printf("%s\tname=%s minimum=%lld maximum=%lld\n",
		       __func__, ext_ctrl.name, ext_ctrl.minimum,
		       ext_ctrl.maximum);
		printf("%s\tstep=%lld default_value=%lld\n",
		       __func__, ext_ctrl.step, ext_ctrl.default_value);
		printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
		       __func__, ext_ctrl.elem_size, ext_ctrl.elems,
		       ext_ctrl.nr_of_dims);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
	}

	memset(&ext_ctrl, 0, sizeof(ext_ctrl));
	ext_ctrl.id = V4L2_CID_MPEG_VIDEO_BITRATE;
	if (ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
		printf("%s error querying ext V4L2_CID_MPEG_VIDEO_BITRATE errno=%d %s\n",
		       __func__, errno, strerror(errno));
	} else {
		printf("%s Got ext_ctrl.id=%#08x type=%d\n",
		       __func__, ext_ctrl.id, ext_ctrl.type);
		printf("%s\tname=%s minimum=%lld maximum=%lld\n",
		       __func__, ext_ctrl.name, ext_ctrl.minimum,
		       ext_ctrl.maximum);
		printf("%s\tstep=%lld default_value=%lld\n",
		       __func__, ext_ctrl.step, ext_ctrl.default_value);
		printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
		       __func__, ext_ctrl.elem_size, ext_ctrl.elems,
		       ext_ctrl.nr_of_dims);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
	}

	memset(&ext_ctrl, 0, sizeof(ext_ctrl));
	ext_ctrl.id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
	if (ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
		printf("%s error querying ext V4L2_CID_MPEG_VIDEO_H264_I_PERIOD errno=%d %s\n",
		       __func__, errno, strerror(errno));
	} else {
		printf("%s Got ext_ctrl.id=%#08x type=%d\n",
		       __func__, ext_ctrl.id, ext_ctrl.type);
		printf("%s\tname=%s minimum=%lld maximum=%lld\n",
		       __func__, ext_ctrl.name, ext_ctrl.minimum,
		       ext_ctrl.maximum);
		printf("%s\tstep=%lld default_value=%lld\n",
		       __func__, ext_ctrl.step, ext_ctrl.default_value);
		printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
		       __func__, ext_ctrl.elem_size, ext_ctrl.elems,
		       ext_ctrl.nr_of_dims);
		printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
	}
}

static void get_ctrls(int fd)
{
	struct v4l2_ext_controls ctrls;
	struct v4l2_ext_control controls[3];
	int i;

	memset(&ctrls, 0, sizeof(ctrls));
	memset(controls, 0, sizeof(controls));

	ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
	ctrls.count = 3;
	ctrls.controls = controls;

	controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
	controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;

	if (ioctl(fd, VIDIOC_G_EXT_CTRLS, &ctrls)) {
		printf("%s error getting VIDIOC_G_EXT_CTRLS errno=%d %s\n",
		       __func__, errno, strerror(errno));
		return;
	}

	debug_printf("%s got %d values back from VIDIOC_G_EXT_CTRLS\n",
		     __func__, ctrls.count);
	for (i = 0; i < ctrls.count; i++) {
		debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
			     __func__, i, controls[i].id,
			     controls[i].size, controls[i].value);
	}
}

static void try_ctrls(int fd, int bitrate, int gop_size, int i_period)
{
	struct v4l2_ext_controls ctrls;
	struct v4l2_ext_control controls[3];
	int i;

	memset(&ctrls, 0, sizeof(ctrls));
	memset(controls, 0, sizeof(controls));

	ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
	ctrls.count = 3;
	ctrls.controls = controls;

	controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	controls[0].value = gop_size;
	controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
	controls[1].value = bitrate;
	controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
	controls[2].value = i_period;

	if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
		printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
		       __func__, ctrls.error_idx, errno, strerror(errno));
		printf("%s Successful commands TRYed:\n", __func__);
		for (i = 0; i < ctrls.error_idx; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	} else {
		printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
		for (i = 0; i < ctrls.count; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	}

#ifdef DEBUG
	printf("%s Testing TRY setting DEF_VAL, EINVAL expected\n", __func__);
	memset(&ctrls, 0, sizeof(ctrls));
	memset(controls, 0, sizeof(controls));

	ctrls.which = V4L2_CTRL_WHICH_DEF_VAL;
	ctrls.count = 3;
	ctrls.controls = controls;

	controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	controls[0].value = 0;
	controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
	controls[1].value = 0;
	controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
	controls[2].value = 0;

	if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
		printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
		       __func__, ctrls.error_idx, errno, strerror(errno));
		for (i = 0; i < ctrls.error_idx; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	} else {
		printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
		for (i = 0; i < ctrls.count; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	}

	printf("%s Testing TRY setting controls[0] to invalid, ERANGE expected with idx=0\n", __func__);
	memset(&ctrls, 0, sizeof(ctrls));
	memset(controls, 0, sizeof(controls));

	ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
	ctrls.count = 3;
	ctrls.controls = controls;

	controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	controls[0].value = 0;
	controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
	controls[1].value = 0;
	controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
	controls[2].value = 0;

	if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
		printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
		       __func__, ctrls.error_idx, errno, strerror(errno));
		for (i = 0; i < ctrls.error_idx; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	} else {
		printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
		for (i = 0; i < ctrls.count; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	}

	printf("%s Testing TRY setting controls[1] to invalid, ERANGE expected with idx=1\n", __func__);
	memset(&ctrls, 0, sizeof(ctrls));
	memset(controls, 0, sizeof(controls));

	ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
	ctrls.count = 3;
	ctrls.controls = controls;

	controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	controls[0].value = 5;
	controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
	controls[1].value = 0;
	controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
	controls[2].value = 0;

	if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
		printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
		       __func__, ctrls.error_idx, errno, strerror(errno));
		for (i = 0; i < ctrls.error_idx; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	} else {
		printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
		for (i = 0; i < ctrls.count; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	}

	printf("%s Testing TRY setting controls[2] to invalid, ERANGE expected with idx=2\n", __func__);
	memset(&ctrls, 0, sizeof(ctrls));
	memset(controls, 0, sizeof(controls));

	ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
	ctrls.count = 3;
	ctrls.controls = controls;

	controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	controls[0].value = 5;
	controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
	controls[1].value = 1000000;
	controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
	controls[2].value = 0;

	if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
		printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
		       __func__, ctrls.error_idx, errno, strerror(errno));
		for (i = 0; i < ctrls.error_idx; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	} else {
		printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
		for (i = 0; i < ctrls.count; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	}
#endif
}

static void set_ctrls(int fd, int bitrate, int gop_size, int i_period)
{
	struct v4l2_ext_controls ctrls;
	struct v4l2_ext_control controls[3];
	int i;

	memset(&ctrls, 0, sizeof(ctrls));
	memset(controls, 0, sizeof(controls));

	ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
	ctrls.count = 3;
	ctrls.controls = controls;

	controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	controls[0].value = gop_size;
	controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
	controls[1].value = bitrate;
	controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
	controls[2].value = i_period;

	if (ioctl(fd, VIDIOC_S_EXT_CTRLS, &ctrls)) {
		printf("%s error getting VIDIOC_S_EXT_CTRLS error_idx=%d errno=%d %s\n",
		       __func__, ctrls.error_idx, errno, strerror(errno));
		if (ctrls.error_idx == ctrls.count) {
			printf("%s S_EXT_CTRLS failed during pre-check\n",
			       __func__);
		} else {
			printf("%s S_EXT_CTRLS failed during set. Controls set before failure:\n",
			       __func__);
			for (i = 0; i < ctrls.error_idx; i++) {
				printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				       __func__, i, controls[i].id,
				       controls[i].size, controls[i].value);
			}
		}
	} else {
		printf("%s VIDIOC_S_EXT_CTRLS succeeded\n", __func__);
		for (i = 0; i < ctrls.count; i++) {
			debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
				     __func__, i, controls[i].id,
				     controls[i].size, controls[i].value);
		}
	}
}

static int subscribe_events(int fd)
{
	struct v4l2_event_subscription sub;

	sub.type = V4L2_EVENT_EOS;
	debug_printf("Calling V4L2 IOCTL VIDIOC_SUBSCRIBE_EVENT\n");
	if (ioctl(fd, VIDIOC_SUBSCRIBE_EVENT, &sub)) {
		printf("Failed to subscribe to events: err: %d %s\n",
				errno, strerror(errno));
		return 1;
	}

	sub.type = V4L2_EVENT_EOS;
	debug_printf("Calling V4L2 IOCTL VIDIOC_SUBSCRIBE_EVENT\n");
	if (ioctl(fd, VIDIOC_SUBSCRIBE_EVENT, &sub)) {
		printf("Failed to subscribe to events: err: %d %s\n",
				errno, strerror(errno));
		return 1;
	}

	return 0;
}

static void unsubscribe_events(int fd)
{
	struct v4l2_event_subscription sub;

	sub.type = V4L2_EVENT_ALL;
	debug_printf("Calling V4L2 IOCTL VIDIOC_UNSUBSCRIBE_EVENT\n");
	if (ioctl(fd, VIDIOC_UNSUBSCRIBE_EVENT, &sub))
		printf("Failed to unsubscribe from events: err: %d %s\n",
				errno, strerror(errno));

	return;
}

static int query_device(int fd)
{
	struct v4l2_capability cap = {0};
	struct v4l2_fmtdesc desc = {0};
	struct v4l2_frmsizeenum frmsize = {0};
	int ret = 0;

	debug_printf("%s Enter\n", __func__);

	ret = ioctl(fd, VIDIOC_QUERYCAP, &cap);
	if (ret) {
		printf("VIDIOC_QUERYCAP failed\n");
		return -1;
	}

	debug_printf("Querycaps: fd=%#x driver=%s card=%s bus_info=%s\n",
		     fd, cap.driver, cap.card, cap.bus_info);
	debug_printf("Querycaps: device_caps=%#08x capabilities=%#08x\n",
		     cap.device_caps, cap.capabilities);

	desc.index = 0;
	desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;

	debug_printf("Calling VIDIOC_ENUM_FMT on CAPTURE\n");
    printf("===========================\n");
	while ((ret = ioctl(fd, VIDIOC_ENUM_FMT, &desc)) == 0) {
		debug_printf("desc.index = %d, pixelformat = %c%c%c%c, desciption = %s\n",
			     desc.index, desc.pixelformat & 0xff,
			     (desc.pixelformat >> 8) & 0xff,
			     (desc.pixelformat >> 16) & 0xff,
			     (desc.pixelformat >> 24) & 0xff,
			     desc.description);
		desc.index++;
	}

    printf("===========================\n");

	desc.index = 0;
	desc.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;

	debug_printf("Calling VIDIOC_ENUM_FMT on OUTPUT\n");
    printf("===========================\n");
	while ((ret = ioctl(fd, VIDIOC_ENUM_FMT, &desc)) == 0) {
		debug_printf("desc.index = %d, pixelformat = %c%c%c%c, desciption = %s\n",
			     desc.index, desc.pixelformat & 0xff,
			     (desc.pixelformat >> 8) & 0xff,
			     (desc.pixelformat >> 16) & 0xff,
			     (desc.pixelformat >> 24) & 0xff,
			     desc.description);
		desc.index++;
	}
    printf("===========================\n");

	frmsize.index = 0;
    frmsize.type = V4L2_FRMSIZE_TYPE_DISCRETE;

	debug_printf("Calling VIDIOC_ENUM_FRAMESIZES\n");
    printf("===========================%d\n", frmsize.type);
	ret = ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize);
	if (ret) {
		printf("VIDIOC_ENUM_FRAMESIZES failed %d %s\n",
		       errno, strerror(errno));
	} else if (frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
		debug_printf("VIDIOC_ENUM_FRAMESIZES got DISCRETE\n");
		debug_printf("frmsizes[%d] width=%d height=%d\n", frmsize.index,
			     frmsize.discrete.width,
			     frmsize.discrete.height);
		frmsize.index++;
		while (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) == 0) {
			debug_printf("frmsizes[%d] width=%d height=%d\n",
				     frmsize.index,
				     frmsize.discrete.width,
				     frmsize.discrete.height);
			frmsize.index++;
		}
	} else {
		debug_printf("VIDIOC_ENUM_FRAMESIZES got %s\n",
			     ((frmsize.type == V4L2_FRMSIZE_TYPE_CONTINUOUS) ?
			      "CONTINUOUS" : "STEPWISE"));
		debug_printf("frmsizes min_width=%d max_width=%d step_width=%d\n",
			     frmsize.stepwise.min_width,
			     frmsize.stepwise.max_width,
			     frmsize.stepwise.step_width);
		debug_printf("frmsizes min_height=%d max_height=%d step_height=%d\n",
			     frmsize.stepwise.min_height,
			     frmsize.stepwise.max_height,
			     frmsize.stepwise.step_height);
	}
	return 0;
}

static int check_framerate(int fd, int *frameival_num, int *frameival_den)
{
	struct v4l2_frmivalenum fival = {0};

	fival.index = 0;

	debug_printf("Calling VIDIOC_ENUM_FRAMEINTERVALS\n");
	if (ioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &fival)) {
		printf("VIDIOC_ENUM_FRAMEINTERVALS failed %d %s\n",
		       errno, strerror(errno));
	} else if (fival.type == V4L2_FRMIVAL_TYPE_DISCRETE) {
		debug_printf("VIDIOC_ENUM_FRAMEINTERVALS got DISCRETE\n");
		debug_printf("fival[%d] numerator=%d denominator=%d framerate=%d\n",
			     fival.index, fival.discrete.numerator,
			     fival.discrete.denominator,
			     (fival.discrete.denominator /
			      fival.discrete.numerator));
		fival.index++;
		while (ioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &fival) == 0) {
			debug_printf("fival[%d] numerator=%d denominator=%d framerate=%d\n",
				     fival.index, fival.discrete.numerator,
				     fival.discrete.denominator,
				     (fival.discrete.denominator /
				      fival.discrete.numerator));
			fival.index++;
		}

		fival.index = 0;
		while (ioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &fival) == 0) {
			if ((fival.discrete.denominator /
			     fival.discrete.numerator) ==
			    (*frameival_den / *frameival_num)) {
				*frameival_num = fival.discrete.numerator;
				*frameival_den = fival.discrete.denominator;
				return 0;
			}
			fival.index++;
		}

	} else {
		debug_printf("VIDIOC_ENUM_FRAMEINTERVALS got %s\n",
			     ((fival.type == V4L2_FRMIVAL_TYPE_CONTINUOUS) ?
			      "CONTINUOUS" : "STEPWISE"));
		debug_printf("fival min: numerator=%d denominator=%d framerate=%d\n",
			     fival.stepwise.min.numerator,
			     fival.stepwise.min.denominator,
			     (fival.stepwise.min.denominator /
			      fival.stepwise.min.numerator));
		debug_printf("fival max: numerator=%d denominator=%d framerate=%d\n",
			     fival.stepwise.max.numerator,
			     fival.stepwise.max.denominator,
			     (fival.stepwise.max.denominator /
			      fival.stepwise.max.numerator));
		debug_printf("fival step: numerator=%d denominator=%d",
			     fival.stepwise.step.numerator,
			     fival.stepwise.step.denominator);

		printf("CONTINUOUS and STEPWISE FRAMEINTERVALS not supported\n");
		return 0;
	}

	printf("Requested frame interval not found, exiting\n");
	return 0;
}

static int get_n_frames(int rdfd, int frame_size)
{
	int size, ret;;
	debug_printf("Discovering number of frames in file\n");
	/*
	 * Seek to end of file to get size, then move file offset
	 * back to beginning of file
	 */
	size = lseek(rdfd, 0, SEEK_END);
    printf("yuv size = %d\n", size);
	lseek(rdfd, 0, SEEK_SET);

	ret = size / frame_size;

	if (size != (frame_size * ret)) {
		printf("Input file does not contain a whole number of frames\n");
		return -1;
	}
	if (0 == ret){
		printf("Input file contains no frames\n");
		return -1;
	}

	debug_printf("Found %d frames in input file\n", ret);
	return ret;
}

static void find_format(char name[], struct tienc_format *format,
			const struct tienc_format formats[], int formats_size)
{
	int i;

	for (i = 0; i < formats_size; i++) {
		if (0 == strcmp(name, formats[i].name)) {
			debug_printf("Found format %s\n", name);
			*format = formats[i];
			break;
		}
	}

	if (i >= formats_size) {
		printf("No format found. Defaulting to %s\n",
		       formats[0].name);
		*format = formats[0];
	}
}

static void calculate_formats(char format_name[], char codec_name[],
			     struct tienc_format *out_format,
			     struct tienc_format *cap_format,
			     int width, int height)
{
	find_format(format_name, out_format, out_formats,
		    (sizeof(out_formats) / sizeof(out_formats[0])));
	find_format(codec_name, cap_format, cap_formats,
		    (sizeof(cap_formats) / sizeof(cap_formats[0])));

	/* Fill user supplied and calculated values */
	out_format->width = width;
	out_format->height = height;
	out_format->stride = ALIGN(width * out_format->bytes_pp, HW_ALIGN);
	out_format->size = ((out_format->stride * height) *
			    out_format->size_num) / out_format->size_den;
    printf("--- out_format stride=%d size=%d w:%d h:%d---\n", out_format->stride, out_format->size, width, height);
    // --- out_format stride=960 size=777600 w:960 h:540---

	cap_format->width = width;
	cap_format->height = height;
	cap_format->stride = width;
	cap_format->size = cap_format->stride * height;
    printf("--- cap_format stride=%d size=%d w:%d h:%d---\n", cap_format->stride, cap_format->size, width, height);
    // --- cap_format stride=960 size=518400 w:960 h:540---
}

#ifdef DRMMODE
int find_drm_device(char *drm_path)
{
	const char *drm_name = "cardx";
	char name[256] = "";
	DIR *d;
	struct dirent *dir;
	drmModeResPtr res;
	int fd;

	d = opendir(drm_path);
	if (!d) {
		printf("Failed to open drm device path %s %d %s\n", drm_path,
		       errno, strerror(errno));
		return -1;
	}

	while ((dir = readdir(d)) != NULL) {
		if (strncmp(dir->d_name, drm_name, 4) == 0) {
			strncpy(name, drm_path, sizeof(name));
			strncat(name, dir->d_name, sizeof(name));
			fd = open(name, O_CLOEXEC | O_RDWR);
			if (fd < 0) {
				printf("Failed to open drm device %s %d %s\n",
				       name, errno, strerror(errno));
				continue;
			}

			res = drmModeGetResources(fd);
			if (res && res->count_crtcs > 0 &&
			    res->count_connectors > 0 &&
			    res->count_encoders > 0) {
				debug_printf("No drm device specified, using %s\n",
					     name);
				break;
			}
			close(fd);
			fd = -1;
		}
	}

	if (fd < 0)
		printf("Failed to find a drm device in %s\n", drm_path);

	return fd;
}
#endif

int find_device(char *dev_path)
{
	const char *dev_name = "videox";
	const char *driver_name = "vxe-enc";
	struct v4l2_capability cap = {0};
	char name[256] = "";
	DIR *d;
	struct dirent *dir;
	int fd = -1;

	d = opendir(dev_path);
	if (!d) {
		printf("Failed to open device path %s %d %s\n", dev_path,
		       errno, strerror(errno));
		return -1;
	}

	while ((dir = readdir(d)) != NULL) {
		if (strncmp(dir->d_name, dev_name, 5) == 0) {
			strncpy(name, dev_path, sizeof(name));
			strncat(name, dir->d_name, sizeof(name));
			fd = open(name, O_RDWR | O_NONBLOCK, 0);
			if (fd < 0) {
				printf("Failed to open device %s %d %s\n",
				       name, errno, strerror(errno));
				continue;
			}

			memset(&cap, 0, sizeof(cap));

			if (ioctl(fd, VIDIOC_QUERYCAP, &cap)) {
				printf("VIDIOC_QUERYCAP failed on device %s %d %s\n",
				       name, errno, strerror(errno));
				close(fd);
				continue;
			}

			if (strcmp((const char *)cap.driver, driver_name) == 0) {
				debug_printf("No device specified, using %s\n",
					     name);
				break;
			}

			close(fd);
			fd = -1;
		}
	}

	if (fd < 0)
		printf("Failed to find device in %s\n", dev_path);

	return fd;
}

int main (int argc, char **argv)
{
	int c, ret;
	char input_file[256] = {0};
	char output_file[256] = {0};
#ifdef DRMMODE
	const char *default_drm_path = "/dev/dri/";
#endif
	const char *default_dev_path = "/dev/";
	char drm_path[256] = "";
	char drm_name[256] = "";
	char dev_path[256] = "";
	char dev_name[256] = "";
	char format_name[8] = {0};
	char codec_name[8] = {0};
	int fd = -1, drmfd = -1, rdfd = -1, wrfd = -1;
	int width = 0, height = 0;
	int n_outbuffers, n_capbuffers;
	int n_frames = 0;
	int bitrate = 500000, gop_size = 1800, i_period = 30;
	int frameival_num = 1, frameival_den = 30;
	struct tienc_buffer outbuffers[MAX_OUTBUFS];
	struct tienc_buffer capbuffers[MAX_CAPBUFS];
	struct tienc_format out_format = {0};
	struct tienc_format cap_format = {0};
	enum v4l2_memory out_memory = V4L2_MEMORY_MMAP;
	enum v4l2_memory cap_memory = V4L2_MEMORY_MMAP;
    struct timeval start, end;

	debug_printf("%s Enter\n", __func__);

	n_capbuffers = MAX_CAPBUFS;
	n_outbuffers = MAX_OUTBUFS;

	while (1) {
		c = getopt(argc, argv, "i:o:d:w:h:f:c:n:b:g:p:r:jkl:m:");
		if (-1 == c)
			break;
		switch (c) {
		case 'i':
			snprintf(input_file, sizeof(input_file),
				 "%s", optarg);
			break;
		case 'o':
			snprintf(output_file, sizeof(output_file),
				 "%s", optarg);
			break;
		case 'd':
			snprintf(dev_name, sizeof(dev_name), "%s", optarg);
			break;
		case 'e':
			snprintf(dev_path, sizeof(dev_path), "%s", optarg);
			break;
		case 'w':
			width = atoi(optarg);
			break;
		case 'h':
			height = atoi(optarg);
			break;
		case 'f':
			snprintf(format_name, sizeof(format_name),
				 "%s", optarg);
			break;
		case 'c':
			snprintf(codec_name, sizeof(codec_name),
				 "%s", optarg);
			break;
		case 'n':
			n_frames = atoi(optarg);
			break;
		case 'b':
			bitrate = atoi(optarg);
			break;
		case 'g':
			gop_size = atoi(optarg);
			break;
		case 'p':
			i_period = atoi(optarg);
			break;
		case 'r':
			frameival_den = atoi(optarg);
			break;
		case 'j':
			out_memory = V4L2_MEMORY_DMABUF;
			break;
		case 'k':
			cap_memory = V4L2_MEMORY_DMABUF;
			break;
		case 'l':
			snprintf(drm_name, sizeof(drm_name), "%s", optarg);
			break;
		case 'm':
			snprintf(drm_path, sizeof(drm_path), "%s", optarg);
			break;
		default:
			printf("Use:\n");
			printf("\t./tienc_encode -i <input_file> -w <width> -h <height> [OPTIONS]\n");
			break;
		}
	}

	/* Validate input args */
	if ((0 == strlen(input_file)) || (0 >= width) || (0 >= height)) {
		printf("Requires input values\n");
		printf("\t./tienc_encode -i <input_file> -w <width> -h <height> [OPTIONS]\n");
		printf("\tOPTIONS:\n");
		printf("\t\t-o <output_file_name>\n");
		printf("\t\t\tDump output stream to file\n");
		printf("\t\t-d <device>\n");
		printf("\t\t\tLocation of device node (ex. /dev/video0)\n");
		printf("\t\t-e <device_path>\n");
		printf("\t\t\tDirectory of device node (ex. /dev/)\n");
		printf("\t\t\tNot needed if -d argument is provided\n");
		printf("\t\t-f <format>\n");
		printf("\t\t\tInput image format. Available formats:\n");
		printf("\t\t\t\tNV12\n");
		printf("\t\t\t\tRGBA\n");
		printf("\t\t-c <codec>\n");
		printf("\t\t\tOutput stream codec. Available codecs\n");
		printf("\t\t\t\tH264\n");
		printf("\t\t-n <n_frames>\n");
		printf("\t\t\tNumber of frames to encode\n");
		printf("\t\t-b <bitrate>\n");
		printf("\t\t\tBitrate in bits per second\n");
		printf("\t\t-g <gop_size>\n");
		printf("\t\t\tIDR frame interval\n");
		printf("\t\t-p <i_period>\n");
		printf("\t\t\tI frame period in H264\n");
		printf("\t\t-r <framerate>\n");
		printf("\t\t\tFramerate in frames per second\n");
		printf("\t\t-j\n");
		printf("\t\t\tUse DMA buffers for output stream\n");
		printf("\t\t-k\n");
		printf("\t\t\tUse DMA buffers for capture stream\n");
		printf("\t\t-l <drm_device_name>\n");
		printf("\t\t\tLocation of drm device (ex. /dev/dri/card0)\n");
		printf("\t\t-m <drm_device-path>\n");
		printf("\t\t\tDirectory of drm device node (ex. /dev/dri/)\n");
		printf("\t\t\tNot needed if -l argument is provided\n");
		ret = -1;
		goto exit_failure;
	}

	calculate_formats(format_name, codec_name, &out_format, &cap_format,
			  width, height);

	out_format.memory = out_memory;
	cap_format.memory = cap_memory;

	if ((out_format.memory == V4L2_MEMORY_DMABUF) ||
	    (cap_format.memory == V4L2_MEMORY_DMABUF)) {
		if (strlen(drm_name)) {
#ifdef DRM
			/* Known DRM node name */
			drmfd = open(drm_name, O_CLOEXEC | O_RDWR);
			if (drmfd < 0) {
				printf("Failed to open drm device %d %s\n",
				       errno, strerror(errno));
				ret = -1;
				goto exit_failure;
			}
#else
			printf("DRM not supported with current build arguments\n");
			ret = -1;
			goto exit_failure;
#endif
		} else {
#ifdef DRMMODE
			/* Search DRM directory */
			if (strlen(drm_path) == 0)
				snprintf(drm_path, sizeof(drm_path), "%s",
					 default_drm_path);

			drmfd = find_drm_device(drm_path);
			if (drmfd < 0) {
				printf("Failed to find drm device\n");
				ret = -1;
				goto exit_failure;
			}
#else
			printf("Can't search DRM directory\n");
			ret = -1;
			goto exit_failure;
#endif
		}
	}

	rdfd = open(input_file, O_RDONLY);
	if (0 > rdfd) {
		printf("Failed to open input file: %s\n", input_file);
		ret = -1;
		goto drm_opened;
	}
	debug_printf("Opened input file: %s\n", input_file);

    printf("=================== width=%d height=%d ==============\n", out_format.width, out_format.height);
	ret = get_n_frames(rdfd, ((out_format.width * out_format.height *
				   out_format.size_num) / out_format.size_den));
	if (0 >= ret)
		goto rdfd_opened;

	if (n_frames == 0) {
		debug_printf("Found %d frames\n", ret);
		n_frames = ret;
	} else if (ret > n_frames) {
		debug_printf("More than %d frames found, limiting to %d frames\n",
			     n_frames, n_frames);
	} else {
		debug_printf("Less than %d frames found, using max of %d frames\n",
			     n_frames, ret);
		n_frames = ret;
	}

	if (strlen(output_file)) {
		wrfd = open(output_file, O_CREAT | O_WRONLY | O_TRUNC, 0777);
		if (0 > wrfd) {
			printf("Failed to open output file: %s\n", output_file);
			ret = -1;
			goto rdfd_opened;
		}
		debug_printf("Outputting to %s\n", output_file);
	}

	if (strlen(dev_name)) {
		fd = open(dev_name, O_RDWR | O_NONBLOCK, 0);
		if (0 > fd) {
			printf("Failed to open device: %s\n", dev_name);
			ret = -1;
			goto wrfd_opened;
		}
		debug_printf("Opened device %s\n", dev_name);
	} else {
		/* Search dev directory */
		if (strlen(dev_path) == 0)
			snprintf(dev_path, sizeof(dev_path), "%s",
				 default_dev_path);

		fd = find_device(dev_path);
		if (fd < 0) {
			printf("Failed to find device\n");
			ret = -1;
			goto wrfd_opened;
		}
	}

	ret = query_device(fd);
	if (ret)
		goto dev_opened;

	/*ret = check_framerate(fd, &frameival_num, &frameival_den);
	if (ret)
		goto dev_opened;*/

	ret = subscribe_events(fd);
	if (ret)
		goto dev_opened;

	query_ctrls(fd);
	get_ctrls(fd);
	try_ctrls(fd, bitrate, gop_size, i_period);
	set_ctrls(fd, bitrate, gop_size, i_period);
	get_ctrls(fd);

	get_parameters(fd);
	ret = set_parameters(fd, frameival_num, frameival_den);
	if (ret)
		goto dev_opened;
	get_parameters(fd);

	get_format(fd, out_format);
	get_format(fd, cap_format);

	try_format(fd, out_format);
	try_format(fd, cap_format);

	ret = set_format(fd, out_format);
	if (ret)
		goto events_subscribed;

	ret = set_format(fd, cap_format);
	if (ret)
		goto events_subscribed;

	get_format(fd, out_format);
	get_format(fd, cap_format);

	ret = alloc_bufs(fd, drmfd, &n_outbuffers, outbuffers,
			 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, out_format);
	if (ret)
		goto bufs_alloced;

	ret = alloc_bufs(fd, drmfd, &n_capbuffers, capbuffers,
			 V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, cap_format);
	if (ret)
		goto bufs_alloced;

    while (1) {
        gettimeofday( &start, NULL );
        ret = streaming_loop(fd, rdfd, wrfd, outbuffers,
                     capbuffers, n_outbuffers,
                     n_capbuffers, out_format, cap_format,
                     n_frames);
        gettimeofday( &end, NULL );
        
        float  timeuse;
        timeuse = 1000000 * ( end.tv_sec - start.tv_sec ) + (end.tv_usec - start.tv_usec); 
		timeuse /= 1000;
		printf("--- encoder timeuse = %f\n", timeuse);

        printf("!23333333333333333333333333333333333\n");
        usleep(30*1000);
        lseek(rdfd, 0, SEEK_SET);

    }
	if (ret)
		goto bufs_alloced;

	ret = 0;

bufs_alloced:
	dealloc_bufs(fd, drmfd, &n_outbuffers, outbuffers, out_format);
	dealloc_bufs(fd, drmfd, &n_capbuffers, capbuffers, cap_format);
events_subscribed:
	unsubscribe_events(fd);
dev_opened:
	close(fd);
wrfd_opened:
	if (0 <= wrfd)
		close(wrfd);
rdfd_opened:
	close(rdfd);
drm_opened:
	close(drmfd);
exit_failure:
	return ret;
}
